import lejos.nxt.*;
import java.lang.*;

/*

  SUMMARY:

  EG 1003 SLDP (Semester-long design project) Autonomous underwater vehicle control program.
  Current capabilities: Open-loop thrust and closed-loop yaw angle (steering) control.
  
  (c) Abhimanyu Ghosh, Polytechnic Institute of New York University (2011 - 2012)

  TODO:
  - Refine angular estimates using better sampling scheme + zero-rate recalibration schemes
  - Improve timing, make it more exact

 */

public class auv_gnc_exp {

	public static void main(String[] args) {

		STGyro s = new STGyro(SensorPort.S2);
		s.init();
		double angle_target = 0;
		double angle_z = 0;
		double average;
		double sum = 0.0;
		int iter = 0;
		for (iter = 0; iter < 300; iter++)
		{
			double k = (double)s.getAngles()[2];
			System.out.println(k); 
			sum += k;
			try {Thread.sleep(15);}
			catch(InterruptedException e) {}
		} 
		average = (double)sum / (double)300; 
		MotorPort.B.controlMotor(100, 1);

		int a[] = new int[3]; // Storage for gyroscope sensor data...

		long time_now = 0;
		long init_time = System.currentTimeMillis(); 

		// Beginning of MASTER while loop

		while(!Button.ENTER.isPressed()) {

		a = s.getAngles();
		time_now = System.currentTimeMillis() - init_time;

		angle_z += ((double)a[2] - average) * 0.000371184 * 1.05;
		// Scripting section
		if((int)time_now >= 5000 && (int)time_now <= 5100) { angle_target = -20; }// Initial roll and reverse maneuver to maintain angle of attack with respect 											  // to the left wall.
		if((int)time_now > 7000 && (int)time_now <= 7100) { angle_target = -4.5; }// Completion of above roll + reverse maneuver.
		if((int)time_now > 50500 && (int)time_now <= 50600) {MotorPort.B.controlMotor(100, 2);}	// Reverse thrust to pull back from the wall and get steering  														// clearance
		if((int)time_now > 66000 && (int)time_now <= 66300) {MotorPort.B.controlMotor(0, 1); angle_target = 2;}
		if((int)time_now > 70000 && (int)time_now <= 70100) {MotorPort.B.controlMotor(100, 1);}
		if((int)time_now > 73500 && (int)time_now <= 73100) {MotorPort.B.controlMotor(100, 2);}
		if((int)time_now > 74000 && (int)time_now <= 74100) {MotorPort.B.controlMotor(0, 1); angle_target = 90;}
		if((int)time_now > 77000 && (int)time_now <= 77100) {MotorPort.B.controlMotor(100, 1);}
		if((int)time_now > 82000 && (int)time_now <= 82100) {MotorPort.B.controlMotor(0, 1); angle_target = 45;}
		if((int)time_now > 85000 && (int)time_now <= 85100) {MotorPort.B.controlMotor(100, 2);}
		if((int)time_now > 96000 && (int)time_now <= 96100) {MotorPort.B.controlMotor(0, 1); angle_target = 179;}
		if((int)time_now > 99000 && (int)time_now <= 99100) {MotorPort.B.controlMotor(100, 1);}
		if((int)time_now > 107000 && (int)time_now > 107100) {angle_target = 180;}


		if(angle_z > 720 || time_now > 155000)  // Option to make a wild number of left turns to end the loop...
		{
			break; 							// CRITICAL LINE... else we end up in an infinite loop!!
		}

		// End of scripting section
		
		if(angle_z > 0){
		MotorPort.A.controlMotor((int)((angle_z + angle_target)*-7.5), 1);} // 6.5
		if(angle_z <= 0){
		MotorPort.A.controlMotor((int)((angle_z + angle_target)*7.5), 2);}

		try {Thread.sleep(15);}
		catch(InterruptedException e) {}

		}
	}
} 

class STGyro extends I2CSensor {

// REMEMBER: 210 is the shifter 7-bit address of the I2C device!!!
// REMINDER TO REMEMBER: MSB (Most significant bit) is the sign bit in Java... must use the "... & 0xFF" operand to get the original, unsigned data!!!

	public STGyro(I2CPort port)
	{
		super(port, 210, MODE_RAW, TYPE_LOWSPEED);
	}

	public void init()
	{
		this.sendData(0x20, (byte)159); // was 15 - SAFE
		this.sendData(0x23, (byte)0); // 32 for 1000 deg/s, 16 for 500 deg/s, 
		this.sendData(0x24, (byte)2); // WAS 2 (SAFE VAL)
	}

	public int[] getAngles()
	{
		int ret[] = new int[3];
		byte x_h[] = new byte[1];
		byte x_l[] = new byte[1];

		byte y_h[] = new byte[1];
		byte y_l[] = new byte[1];

		byte z_h[] = new byte[1];
		byte z_l[] = new byte[1];
		
		if(this.getData(0x2A, y_l, 1) == 0){
		this.getData(0x2B, y_h, 1);}

		if(this.getData(0x28, x_l, 1) == 0){
		this.getData(0x29, x_h, 1);}

		if(this.getData(0x2C, z_l, 1) == 0){
		this.getData(0x2D, z_h, 1);}

		ret[0] = ((x_h[0] << 8) | x_l[0]);
		ret[1] = ((y_h[0] << 8) | y_l[0]);
		ret[2] = ((z_h[0] << 8) | z_l[0]&0xFF);

		this.sendData(0x2E, (byte)0);			
		return ret;
	}

}
